#include "csgnode.h"

CSGNode::CSGNode(CSGNode *parent )
    :m_parent(parent)
{
    m_visible = true;
}

CSGIntersection::CSGIntersection( float t1, float t2,  glm::vec4 normal1, glm::vec4 normal2, CSGPrimitive *primitive1, CSGPrimitive *primitive2)
{
    if ( t1 < t2 ) {
        t[0] = t1;
        t[1] = t2;
        normal[0] = normal1;
        normal[1] = normal2;
        primitive[0] = primitive1;
        primitive[1] = primitive2;
    } else {
        t[0] = t2;
        t[1] = t1;
        normal[0] = normal2;
        normal[1] = normal1;
        primitive[0] = primitive2;
        primitive[1] = primitive1;
    }
}

bool CSGIntersection::do_overlap(CSGIntersection &intersection)
{
    // BOTH CASES TEST
    if ( ( (intersection.t[0] <= t[0]) && (t[0] <= intersection.t[1]) ) &&
         ( (intersection.t[0] <= t[1]) && (t[1] <= intersection.t[1]) ) ) {
    }
    if ( (t[0] <= intersection.t[0])  && (intersection.t[0] <= t[1]) ) {
        return true;
    }
    if ( (t[0] <= intersection.t[1])  && (intersection.t[1] <= t[1]) ) {
        return true;
    }
    if ( (intersection.t[0] <= t[0]) && (t[0] <= intersection.t[1]) ) {
        return true;
    }
    if ( (intersection.t[0] <= t[1]) && (t[1] <= intersection.t[1]) ) {
        return true;
    }

    return false;
}

// Add a new son and consequently set this node as the son's parent
void CSGNode::add_son(QSharedPointer<CSGNode> node)
{
    node->set_parent(this);
    m_sons.push_back(node);
}

// Propagate an event update matrixes accums/ops -- should be called only once per operation execution on
// some primitive/node e.g. a translation in a object, call this before traversing the tree
void CSGNode::update_matrix()
{
    if ( !m_parent.isNull() ) {
        m_inv_accum_operation = m_parent->get_accumulated_inverse_matrix();
    }
    // Propagate update to all sons
    for (unsigned int i = 0; i < m_sons.size(); i++) {
        m_sons[i]->update_matrix();
    }
}

// Returns this node accumulated inverse matrix
const glm::mat4 & CSGNode::get_accumulated_inverse_matrix()
{
    return m_inv_accum_operation;
}

// Getter/Setter to obtain/set this node's parent
QSharedPointer<CSGNode> CSGNode::get_parent()
{
    return m_parent;
}

void CSGNode::set_parent(CSGNode *ptr)
{
    m_parent = QSharedPointer<CSGNode>(ptr);
}

// Accordingly to the given type, we will instantiate an appropriate Primitive
CSGPrimitive::CSGPrimitive( PrimitiveType  type, NodeType node_type, int id )
    : m_type(type)
{
    m_material = new Material();
    m_node_type = node_type;
    m_id = id;
    m_top_node = false;
    switch(m_type) {
    case SPHERE:
        m_primitive = new Sphere();
        break;
    case CYLINDER:
        m_primitive = new Cylinder();
        break;
    case CONE:
        m_primitive = new Cone();
        break;
    case CUBE:
        m_primitive = new Cube();
        break;
    default:
        break;
    }
    m_visible = true;
}

// In this case, it is just call Primitive->get_intersection(ray) -- or something like that
const std::list<CSGIntersection> & CSGPrimitive::set_membership(const Ray &ray)
{
    m_intersections.clear();
    Ray new_ray = ray;
    new_ray.m_direction = glm::normalize(ray.m_direction);
    if ( (m_primitive != NULL) && m_primitive->ray_collision(ray) ) {
        CSGIntersection intersection( m_primitive->t[0], m_primitive->t[1],
                                      // Normal space transpose(inverse(object_operation))
                                      //			glm::transpose(m_inv_accum_operation)*m_primitive->normal[0],
                                      //			glm::transpose(m_inv_accum_operation)*m_primitive->normal[1], this, this );
                                      m_primitive->normal[0],
                                      m_primitive->normal[1], this, this );
        m_intersections.push_back(intersection);
    }
    return m_intersections;
}



CSGBooleanOperation::CSGBooleanOperation( OperationType type, NodeType node_type, int id  )
    :m_type(type)
{
    m_node_type = node_type;
    m_id = id;
    m_top_node = false;
}

const std::list<CSGIntersection> & CSGBooleanOperation::set_membership(const Ray &ray)
{
    m_intersections.clear();
    if ( m_sons.size() < 2 ) {	// Less than 2 intersection list
//        qDebug() << "Less than 2 intersection list ";
//        qDebug() << "File: " << __FILE__;
//        qDebug() << "Line: " << __LINE__;
    } else {
        const std::list<CSGIntersection> &results_left = m_sons[0]->set_membership(ray);
        const std::list<CSGIntersection> &results_right = m_sons[1]->set_membership(ray);

        switch(m_type)
        {
        case UNION:
            perform_union(results_left, results_right);
            break;
        case INTERSECTION:
            perform_intersection(results_left, results_right);
            break;
        case DIFFERENCE:
            perform_difference(results_left, results_right);
            break;
        default:
            break;
        }
    }
    return m_intersections;
}

void CSGBooleanOperation::perform_union( const std::list<CSGIntersection> &first, const std::list<CSGIntersection> &second_par )
{
    float t1, t2;
    glm::vec4 normal1, normal2;
    std::list<CSGIntersection>::iterator first_iter;
    std::list<CSGIntersection>::iterator second_iter;
    CSGPrimitive *primitive1, *primitive2;

    // Copy first list to m_intersections -- if there is a intersection with an element from list 2, we will
    // replace the current element of m_intersections with the union of these two elements, and remove the
    // respective element from the second list
    // So, by the end of the algorithm, we will have elements that dont intersect with each other in list one and two
    // then, we just add the remaining elements from list two to the list one
    m_intersections = first;

    if ( first.empty() ) {
        // since we are in a union
        m_intersections = second_par;
        return;
    }

    std::list<CSGIntersection> second = second_par;

    for ( first_iter = m_intersections.begin(); first_iter != m_intersections.end(); first_iter++ ) {
        if ( second.empty() ) {
            break;
        }
        for ( second_iter = second.begin(); second_iter != second.end(); ) {
            if ( first_iter->do_overlap(*second_iter) ) {
                // the smallest from two
                if ( first_iter->t[0] < second_iter->t[0] ) {
                    t1 = first_iter->t[0];
                    normal1 = first_iter->normal[0];
                    primitive1 = first_iter->primitive[0];
                } else {
                    t1 = second_iter->t[0];
                    normal1 = second_iter->normal[0];
                    primitive1 = second_iter->primitive[0];
                }
                // the greatest from two
                if ( first_iter->t[1] < second_iter->t[1] ) {
                    t2 = second_iter->t[1];
                    normal2 = second_iter->normal[1];
                    primitive2 = second_iter->primitive[1];
                } else {
                    t2 = first_iter->t[1];
                    normal2 = first_iter->normal[1];
                    primitive2 = first_iter->primitive[1];
                }
                first_iter->t[0] = t1;
                first_iter->t[1] = t2;
                first_iter->normal[0] = normal1;
                first_iter->normal[1] = normal2;
                first_iter->primitive[0] = primitive1;
                first_iter->primitive[1] = primitive2;
                second.erase(second_iter++);
            } else {
                second_iter++;
            }
        }
    }
    // Copy the remaining elements from list two - i.e. those that dont have any intersection
    m_intersections.insert(m_intersections.end(), second.begin(), second.end());
}

void CSGBooleanOperation::perform_intersection( const std::list<CSGIntersection> &first, const std::list<CSGIntersection> &second_par )
{
    float t1, t2;
    glm::vec4 normal1, normal2;
    std::list<CSGIntersection>::iterator first_iter;
    std::list<CSGIntersection>::iterator second_iter;
    CSGPrimitive *primitive1, *primitive2;

    // Copy first list to m_intersections -- if there is a intersection with an element from list 2, we will
    // replace the current element of m_intersections with the union of these two elements, and remove the
    // respective element from the second list
    // So, by the end of the algorithm, we will have elements that dont intersect with each other in list one and two
    // then, we just add the remaining elements from list two to the list one

    if ( first.empty() || second_par.empty() ) {
        return;
    }

    m_intersections = first;
    std::list<CSGIntersection> second = second_par;

    bool intersection_happened = false;

    for ( first_iter = m_intersections.begin(); first_iter != m_intersections.end(); ) {
        intersection_happened = false;
        if ( second.empty() ) {
            break;
        }
        for ( second_iter = second.begin(); second_iter != second.end(); ) {
            if ( first_iter->do_overlap(*second_iter) ) {
                // the greatest from two
                if ( first_iter->t[0] >= second_iter->t[0] ) {
                    t1 = first_iter->t[0];
                    normal1 = first_iter->normal[0];
                    primitive1 = first_iter->primitive[0];
                } else {
                    t1 = second_iter->t[0];
                    normal1 = second_iter->normal[0];
                    primitive1 = second_iter->primitive[0];
                }
                // the smallest from two
                if ( first_iter->t[1] >= second_iter->t[1] ) {
                    t2 = second_iter->t[1];
                    normal2 = second_iter->normal[1];
                    primitive2 = second_iter->primitive[1];
                } else {
                    t2 = first_iter->t[1];
                    normal2 = first_iter->normal[1];
                    primitive2 = first_iter->primitive[1];
                }
                first_iter->t[0] = t1;
                first_iter->t[1] = t2;
                first_iter->normal[0] = normal1;
                first_iter->normal[1] = normal2;
                first_iter->primitive[0] = primitive1;
                first_iter->primitive[1] = primitive2;
                second.erase(second_iter++);
                intersection_happened = true;
            } else {
                second_iter++;
            }
        }
        if ( !intersection_happened ) {
            m_intersections.erase(first_iter++);	// intersection not happened - just remove the intersection
        } else {
            first_iter++;			// intersection happened - dont remove the new intersection
        }
    }
}

void CSGBooleanOperation::perform_difference( const std::list<CSGIntersection> &first, const std::list<CSGIntersection> &second_par )
{

    std::list<CSGIntersection>::iterator first_iter;
    std::list<CSGIntersection>::iterator second_iter;

    // Copy first list to m_intersections -- if there is a intersection with an element from list 2, we will
    // replace the current element of m_intersections with the union of these two elements, and remove the
    // respective element from the second list
    // So, by the end of the algorithm, we will have elements that dont intersect with each other in list one and two
    // then, we just add the remaining elements from list two to the list one
    m_intersections = first;
    bool first_removed = false;
    std::list<CSGIntersection> second = second_par;

    for ( first_iter = m_intersections.begin(); first_iter != m_intersections.end(); first_iter++ ) {
        if ( first_removed ) {
            first_iter--;
        }
        first_removed = false;
        for ( second_iter = second.begin(); second_iter != second.end(); second_iter++ ) {
            if ( first_iter->do_overlap(*second_iter) ) {
                bool fir_verify = first_iter->t[0] <= second_iter->t[0];
                bool sec_verify = first_iter->t[1] >= second_iter->t[0];
                bool thi_verify = first_iter->t[0] <= second_iter->t[1];
                bool fou_verify = first_iter->t[1] >= second_iter->t[1];

                // First case - both points of the secont_iter are inside the first_iter interval
                if ( fir_verify && sec_verify && thi_verify && fou_verify ) {
                    first_iter->t[0] = first_iter->t[0];
                    first_iter->t[1] = second_iter->t[0];
                    first_iter->normal[0] = first_iter->normal[0];
                    first_iter->normal[1] = -second_iter->normal[0];
                    first_iter->primitive[0] = first_iter->primitive[0];
                    first_iter->primitive[1] = second_iter->primitive[0];
                    // If we insert it after the current position it would be better
                    // We could just pick the first intersection for raycasting for example
                    m_intersections.push_front(CSGIntersection(
                                                   second_iter->t[1],
                                                   first_iter->t[1],
                                                   -second_iter->normal[1],
                                                   first_iter->normal[1],
                                                   second_iter->primitive[1],
                                                   first_iter->primitive[1]
                                                   ));
                } else if (fir_verify && sec_verify) {	// Second case, the first point of second interval is inside first_interval
                    first_iter->t[1] = second_iter->t[0];
                    first_iter->normal[1] = -second_iter->normal[0];
                    first_iter->primitive[1] = second_iter->primitive[0];
                } else if (thi_verify && fou_verify) { // Third case, second point of second interval is inside first_interval
                    first_iter->t[0] = second_iter->t[1];
                    first_iter->normal[0] = -second_iter->normal[1];
                    first_iter->primitive[0] = second_iter->primitive[1];
                } else {	// Fourth case - interval 1 inside interval 2
                    m_intersections.erase(first_iter++);
                    first_removed = true;
                    break;
                }
            }
        }
    }
}

CSGGeometricOperation::CSGGeometricOperation( OperationType type, NodeType node_type, int id )
    :m_type(type)
{
    m_node_type = node_type;
    m_id = id;
    m_top_node = false;
    m_visible = true;
}

const std::list<CSGIntersection> & CSGGeometricOperation::set_membership(const Ray &ray)
{
    // Less than one son?
    if ( m_sons.size() < 1 ) {
//        qDebug() << "ERROR - LESS THAN ONE SON";
//        qDebug() << "File: " << __FILE__;
//        qDebug() << "Line: " << __LINE__;
    } else {
        Ray new_ray = ray;
        if ( m_type != TRANSLATION ) {
            new_ray.m_direction = m_inv_matrix*ray.m_direction;
        }
        new_ray.m_origin = m_inv_matrix*ray.m_origin;
        return m_sons[0]->set_membership( new_ray );
    }
}

void CSGGeometricOperation::perform_translation( glm::vec3 & tra_vector )
{
    if ( m_type == TRANSLATION ) {
        // Concatenate this new translation with the so far translation
        // Remembering that (A.B)^-1 = B^-1 A^-1
        m_inv_matrix = glm::gtx::transform::translate(-tra_vector)*m_inv_matrix;
    }
}

void CSGGeometricOperation::perform_rotation( glm::vec3 & rot_vector, float angle )
{
    if ( m_type == ROTATION ) {
        m_inv_matrix = glm::gtx::transform::rotate(-angle, rot_vector)* m_inv_matrix;
    }
}

void CSGGeometricOperation::perform_scale( glm::vec3 & sca_factor )
{
    if ( m_type == SCALE ) {
        m_inv_matrix = glm::gtx::transform::scale(1.0f/sca_factor.x, 1.0f/sca_factor.y, 1.0f/sca_factor.z)* m_inv_matrix;
    }
}

// Will update accum inv matrix to m_inv_matrix*(this node parent accum inverse)
void CSGGeometricOperation::update_matrix()
{
    if ( !m_parent.isNull() ) {
       //qDebug() << "NOT NULL PARENT";
        // Accum with this node's parent acumm matrix -- if this node has a parent
        m_inv_accum_operation = m_inv_matrix*m_parent->get_accumulated_inverse_matrix();
    } else {
        //qDebug() << "NULL PARENT";
        // No parent, just this plain operation so far inv matrix
        m_inv_accum_operation = m_inv_matrix;
    }
    //qDebug() << "Operation type " << m_type;
    // Propagate update to all sons	-- Should be only one
    for (unsigned int i = 0; i < m_sons.size(); i++) {
        m_sons[i]->update_matrix();
    }
}
